#!/usr/bin/python3

import rospy
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, PointField
import pcl
# import pcl_ros
import numpy as np
import std_msgs.msg

from sklearn.decomposition import PCA


def cal_normal(points):
# # 生成示例点云数据
# points = np.array([
#     [1, 0, 0],
#     [2, 0, 0],
#     [3, 0, 0],
#     [1, 1, 0],
#     [2, 1, 0],
#     [3, 1, 0],
#     [1, 2, 0],
#     [2, 2, 0],
#     [3, 2, 0]
# ])

    # 进行PCA分析
    pca = PCA(n_components=3)
    pca.fit(points)

    # 平面法线是PCA中最小特征值对应的特征向量
    normal = pca.components_[-1]
    a = pca.components_[0]
    b = pca.components_[1]
    # 计算法线的方向(这里需要确定法向量的正负也即方向，不然方向会一直变化)
    # print(pca.components_)
    # 归一化法线向量
    normal = normal / np.linalg.norm(normal)
    a = a / np.linalg.norm(a)
    b = b / np.linalg.norm(b)

    print("PCA components:")
    print("{:.2f} {:.2f} {:.2f}".format(a[0], a[1], a[2]))
    print("{:.2f} {:.2f} {:.2f}".format(b[0], b[1], b[2]))
    print("Plane normal:{:.2f}, {:.2f}, {:.2f}".format(normal[0], normal[1], normal[2]))
    # ROS_INFO("Plane normal:{}".format(normal))

    box_link_x = np.array(a).reshape(-1, 1)
    box_link_y = np.array(b).reshape(-1, 1)
    box_link_z = np.array(normal).reshape(-1, 1)
    cal_box_pose(box_link_x, box_link_y, box_link_z)
    return pca.components_


def cal_box_pose(box_link_x, box_link_y, box_link_z):
    box_link_matrix = np.c_[box_link_x, box_link_y, box_link_z]
    camera_link_matrix = np.eye(3)
    camera_link_x = camera_link_matrix[:,[0]]
    camera_link_y = camera_link_matrix[:,[1]]
    camera_link_z = camera_link_matrix[:,[2]]

    R = np.array([
        [box_link_x.T@camera_link_x, box_link_x.T@camera_link_y, box_link_x.T@camera_link_z],
        [box_link_y.T@camera_link_x, box_link_y.T@camera_link_y, box_link_y.T@camera_link_z],
        [box_link_z.T@camera_link_x, box_link_z.T@camera_link_y, box_link_z.T@camera_link_z]
    ])
    # 下面的是ZYX欧拉角来描述旋转的计算（beta ！= 90）
    pitch = np.arctan2(-R[2, 0], np.sqrt(R[0, 0]**2 + R[1, 0]**2))
    yaw = np.arctan2(R[1, 0], R[0, 0])
    roll = np.arctan2(R[2, 1], R[2, 2])

    # print(f"Roll:{roll} pitch:{pitch} yaw:{yaw}")

    print(f"Roll:{roll[0, 0]:.2f} pitch:{pitch[0, 0]:.2f} yaw:{yaw[0, 0]:.2f}")
    return roll, pitch, yaw

# from pcl_ros import point_cloud2


def generate_point_cloud():
    # 创建一个点云对象
    cloud = pcl.PointCloud()
    points_list = []
    for x in range(0, 45, 1):
        for z in range(0, 27, 1):
            # points_list.append([x*0.1/2, 0, z*0.1/2])
            # 加上一些噪声
            points_list.append([x*0.1/2+np.random.normal(0, 0.01), 0 + np.random.normal(0, 0.01), z*0.1/2+np.random.normal(0, 0.01)])

    # points = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]], dtype=np.float32)
    points = np.array(points_list, dtype=np.float32)
    # ROS_INFO("法向量{}"% cal_normal(points))
    cal_normal(points)

    cloud.from_array(points)

    # 添加点（这里以简单的点为例）
    # cloud.append([1.0, 2.0, 3.0])
    # cloud.append([4.0, 5.0, 6.0])

    # 将PCL点云转换为ROS消息格式
    # ros_cloud = pcl_ros.convert_pcl_to_ros(cloud)
    # ros_cloud = point_cloud2.create_cloud_xyz32(ros_cloud.header, pcl_cloud.to_array())
    # ros_cloud = cloud
        # 创建一个PCL点云对象
    # cloud = pcl.PointCloud()
    # cloud.from_array([[4.0, 5.0, 6.0, 0.0], [7.0, 8.0, 9.0, 1.0]])
    # 定义ROS点云字段
    fields = [
        PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
        PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
        PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
        PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1)
    ]
    # 提取点云数据
    points_list = []
    for data in cloud:
        points_list.append([data[0], data[1], data[2], 1])

    # 创建ROS点云消息
    header = std_msgs.msg.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = 'map'  # 你可以根据需要更改frame_id
    ros_cloud = pc2.create_cloud(header, fields, points_list)

    return ros_cloud

def publish_point_cloud():
    rospy.init_node('point_cloud_publisher')
    pub = rospy.Publisher('/point_cloud', PointCloud2, queue_size=10)
    rate = rospy.Rate(1)  # 1 Hz

    while not rospy.is_shutdown():
        ros_cloud = generate_point_cloud()
        pub.publish(ros_cloud)
        rate.sleep()


if __name__ == '__main__':
    try:
        publish_point_cloud()
    except rospy.ROSInterruptException:
        pass

